import socket, struct, time, threading


class HeartBeat():
    def __init__(self):
        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.send_addr = ('192.168.1.120', 43893)
        self.heart_task = threading.Thread(target=self.send_heartbeat)
        self.heart_task.start()

    def send_heartbeat(self):
        while True:
            data = struct.pack("<3i", 0x21040001, 0, 0)
            self.udp_socket.sendto(data, self.send_addr)
            time.sleep(0.5)  #sending command frequency not lower than 2HZ


class ControlRobot:
    def __init__(self, ip='192.168.1.120', port=43893):
        self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.send_addr = (ip, port)
        self.heartbeat = HeartBeat()
        self.is_stand = False  # 是否站立状态
        self.motion_mode = False  # 是否在移动模式

        time.sleep(1)  # 等待连接稳定
    def send_data(self, code, value, type):
        try:
            data = struct.pack("<3i", code, value, type)
            self.udp_socket.sendto(data, self.send_addr)
            time.sleep(1)  # 等待状态切换完成
            return True
        except Exception as e:
            print(f"发送命令失败: {e}")
            return False

    # 基础控制命令
    def stand_up_or_lie_down(self):
        """切换站立/趴下状态"""
        if self.is_stand:
            self.is_stand = False
            print("切换到趴下状态")
        else:
            self.is_stand = True
            print("切换到站立状态")
        # 发送站立/趴下命令
        if self.send_data(0x21010202, 0, 0):
            print("执行站立/趴下命令")
             

    def soft_emergency_stop(self):
        """软急停"""
        if self.send_data(0x21020C0E, 0, 0):
            print("执行软急停")
             

    def zero_position(self):
        """回零位"""
        if self.send_data(0x21010C05, 0, 0):
            print("执行回零位")
             

    # 姿态调整命令（原地模式）
    def adjust_roll(self, value):
        """调整横滚角"""
        if self.send_data(0x21010131, value, 0):
            print(f"调整横滚角，值：{value}")
             

    def adjust_pitch(self, value):
        """调整俯仰角"""
        if self.send_data(0x21010130, value, 0):
            print(f"调整俯仰角，值：{value}")
             

    def adjust_height(self, value):
        """调整身体高度"""
        if self.send_data(0x21010102, value, 0):
            print(f"调整身体高度，值：{value}")
             

    def adjust_yaw(self, value):
        """调整偏航角"""
        if self.send_data(0x21010135, value, 0):
            print(f"调整偏航角，值：{value}")
             

    # 移动控制命令
    def move_side(self, value):
        """左右平移"""
        if self.send_data(0x21010131, value, 0):
            print(f"左右平移，速度值：{value}")
             

    def move_forward(self, value):
        """前后移动"""
        if self.send_data(0x21010130, value, 0):
            print(f"前后移动，速度值：{value}")
             

    def turn(self, value):
        """转向"""
        if self.send_data(0x21010135, value, 0):
            print(f"转向，速度值：{value}")
             

    # 模式切换命令
    def on_spot_mode(self):
        """切换到原地模式"""
        if self.send_data(0x21010D05, 0, 0):
            print("切换到原地模式")
            self.motion_mode = False
             

    def move_mode(self):
        """切换到移动模式"""
        if self.send_data(0x21010D06, 0, 0):
            print("切换到移动模式")
            self.motion_mode = True
             

    # 步态控制命令
    def flat_low_speed_gait(self):
        """平地低速步态"""
        if self.send_data(0x21010300, 0, 0):
            print("切换到平地低速步态")
             

    def flat_medium_speed_gait(self):
        """平地中速步态"""
        if self.send_data(0x21010307, 0, 0):
            print("切换到平地中速步态")
             

    def flat_high_speed_gait(self):
        """平地高速步态"""
        if self.send_data(0x21010303, 0, 0):
            print("切换到平地高速步态")
             

    # 特殊动作命令
    def twist_body(self):
        """扭身体"""
        if self.send_data(0x21010204, 0, 0):
            print("执行扭身体动作")
             

    def flip(self):
        """翻身"""
        if self.send_data(0x21010205, 0, 0):
            print("执行翻身动作")
             

    def space_step(self):
        """太空步"""
        if self.send_data(0x2101030C, 0, 0):
            print("执行太空步")
             

    def backflip(self):
        """后空翻"""
        if self.send_data(0x21010502, 0, 0):
            print("执行后空翻")
             

    def greet(self):
        """打招呼"""
        if self.send_data(0x21010507, 0, 0):
            print("执行打招呼动作")
             

    # 系统控制命令
    def autonomous_mode(self):
        """切换到自主模式"""
        if self.send_data(0x21010C03, 0, 0):
            print("切换到自主模式")
             

    def manual_mode(self):
        """切换到手动模式"""
        if self.send_data(0x21010C02, 0, 0):
            print("切换到手动模式")
            
    # AI功能控制
    def close_all_ai_options(self):
        """关闭所有AI选项"""
        if self.send_data(0x21012109, 0x00, 0):
            print("关闭所有AI功能")
             

    def enable_obstacle_stop(self):
        """开启停障"""
        if self.send_data(0x21012109, 0x20, 0):
            print("开启停障功能")
             

    def enable_follow(self):
        """开启跟随"""
        if self.send_data(0x21012109, 0xC0, 0):
            print("开启跟随功能")
             
    